﻿using UnityEngine;
using System.Collections;

public struct PhysicContact
{
    public Vector3 ContactPos;
    public Vector3 Normal;
    public PhysicCollider A;
    public PhysicCollider B;
    public float PenetrationDepth;
    public Vector3 RelativeBodyPosA;
    public Vector3 RelativeBodyPosB;
}

public struct Triangle
{
    public Vector3 a, b, c;
    public Triangle(Vector3 a1, Vector3 b1, Vector3 c1)
    {
        a = a1;
        b = b1;
        c = c1;
    }
}

public struct Plane
{
    public Vector3 normal;
    public float distance;
}

public struct Line
{
    public Vector3 start;
    public Vector3 end;
    public Line(Vector3 a, Vector3 b)
    {
        start = a;
        end = b;
    }
}

public class PhysicCollision  {
    public readonly static float EPSILON = 0.000001f;
    private static bool SphereMesh(SphereShapeCollider sphereCol, DynamicMeshCollider meshCol, out PhysicContact contact)
    {
        contact = new PhysicContact();
        var spherePos = sphereCol.transform.position;
        var meshPos = meshCol.transform.position;
        var radius = sphereCol.Radius;
        float minDist = 999 * 999;
        PhysicContact minResult = new PhysicContact();
        var find = false;

        foreach (var tri in meshCol.GetTriangles())
        {
            PhysicContact con;
            //物理检测不要有模型缩放 否则sphere半径不对
            //var relativeSpherePos = Math.InverseTransformPointUnscaled(meshCol.transform, spherePos);
            //var relativeSpherePos = meshCol.transform.InverseTransformPoint(spherePos);
            var ret = DetectSphereTriangle(spherePos, radius, tri, out con);
            if (ret)
            {
                find = true;
                var sqDist = (con.ContactPos - spherePos).sqrMagnitude;
                if (sqDist < minDist)
                {
                    minDist = sqDist;
                    con.A = sphereCol;
                    con.B = meshCol;
                    con.RelativeBodyPosA = con.ContactPos - spherePos;
                    con.RelativeBodyPosB = con.ContactPos - meshPos;
                    minResult = con;
                }
            }
        }
        if (find)
        {
            contact = minResult;
            return true;
        }
        return false;
    }

    public static bool Detect(PhysicCollider col1, PhysicCollider col2, out PhysicContact contact)
    {
        contact = new PhysicContact();
        if(col1.IsFixed && col2.IsFixed)
        {
            return false;
        }

        if(col1.colliderType == PhysicCollider.ColliderType.Sphere && col2.colliderType == PhysicCollider.ColliderType.DynamicMesh)
        {
            var sphereCol = (SphereShapeCollider)col1;
            var meshCol = (DynamicMeshCollider)col2;
            return SphereMesh(sphereCol, meshCol, out contact);
        }else if(col1.colliderType == PhysicCollider.ColliderType.DynamicMesh && col2.colliderType == PhysicCollider.ColliderType.Sphere)
        {
            var sphereCol = (SphereShapeCollider)col2;
            var meshCol = (DynamicMeshCollider)col1;
            return SphereMesh(sphereCol, meshCol, out contact);
        }

        contact = new PhysicContact();
        return false;
    }

    //https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
    //https://www.scratchapixel.com/lessons/3d-basic-rendering/ray-tracing-rendering-a-triangle/moller-trumbore-ray-triangle-intersection
    public static bool DetectRayCastTriangle(Vector3 orig, Vector3 dir, Vector3 v0, Vector3 v1, Vector3 v2, out PhysicContact contact)
    {
        PhysicContact con = new PhysicContact();
        contact = con;

        dir.Normalize();
        var v0v1 = v1 - v0;
        var v0v2 = v2 - v0;
        var pvec = Vector3.Cross(dir, v0v2);
        var det = Vector3.Dot(v0v1, pvec);

        //det < 0 三角面背面相交 忽略碰撞，只和正面三角形碰撞
        if(det < EPSILON)
        {
            return false;
        }

        var invDet = 1 / det;
        var tvec = orig - v0;
        //u, v v0v1 v0v2 计算相交点向量参数
        var u = Vector3.Dot(tvec, pvec) * invDet;
        if(u < 0 ||  u > 1)
        {
            return false;
        }

        var qvec = Vector3.Cross(tvec, v0v1);
        var v = Vector3.Dot(dir, qvec) * invDet;
        if(v < 0 || (u+v) > 1)
        {
            return false;
        }

        //相交点距离
        var t = Vector3.Dot(v0v2, qvec) * invDet;
        if(t > EPSILON)
        {
            //con.Distance = t;
            con.ContactPos = orig + dir * t;
            con.Normal = Vector3.Cross(v0v1.normalized, v0v2.normalized);
            contact = con;
            return true;
        }
        return false;
    }

    private static Plane FromTriangle(Triangle t)
    {
        Plane result;
        result.normal =  Vector3.Cross(t.b - t.a, t.c - t.a).normalized;
        result.distance = Vector3.Dot(result.normal, t.a);
        return result;
    }

    private static Vector3 ClosetPoint(Plane plane, Vector3 point)
    {
        // This works assuming plane.Normal is normalized, which it should be
        float distance = Vector3.Dot(plane.normal, point) - plane.distance;
        // If the plane normal wasn't normalized, we'd need this:
        // distance = distance / DOT(plane.Normal, plane.Normal);

        return point - plane.normal * distance;
    }

    private static Vector3 ClosetPoint(Line line, Vector3 point)
    {
        var lVec = line.end - line.start; // Line Vector
                                           // Project "point" onto the "Line Vector", computing:
                                           // closest(t) = start + t * (end - start)
                                           // T is how far along the line the projected point is
        float t = Vector3.Dot(point - line.start, lVec) / Vector3.Dot(lVec, lVec);
        // Clamp t to the 0 to 1 range
        t = Mathf.Max(t, 0.0f);
        t = Mathf.Min(t, 1.0f);
        // Return projected position of t
        return line.start + lVec * t;
    }

    private static bool PointInTriangle(Vector3 p, Triangle t)
    {
        // Move the triangle so that the point is  
        // now at the origin of the triangle
        var a = t.a - p;
        var b = t.b - p;
        var c = t.c - p;

        // The point should be moved too, so they are both
        // relative, but because we don't use p in the
        // equation anymore, we don't need it!
        // p -= p; // This would just equal the zero vector!

        var normPBC = Vector3.Cross(b, c); // Normal of PBC (u)
        var normPCA = Vector3.Cross(c, a); // Normal of PCA (v)
        var normPAB = Vector3.Cross(a, b); // Normal of PAB (w)

        // Test to see if the normals are facing 
        // the same direction, return false if not
        if (Vector3.Dot(normPBC, normPCA) < 0.0f)
        {
            return false;
        }
        else if (Vector3.Dot(normPBC, normPAB) < 0.0f)
        {
            return false;
        }

        // All normals facing the same way, return true
        return true;
    }

    private static Vector3 ClosetPoint(Triangle t, Vector3 p)
    {
        var plane = FromTriangle(t);
        var closet = ClosetPoint(plane, p);
        if(PointInTriangle(closet, t))
        {
            return closet;
        }

        var c1 = ClosetPoint(new Line(t.a, t.b), closet);
        var c2 = ClosetPoint(new Line(t.b, t.c), closet);
        var c3 = ClosetPoint(new Line(t.c, t.a), closet);

        var magSq1 = (closet - c1).sqrMagnitude;
        var magSq2 = (closet - c2).sqrMagnitude;
        var magSq3 = (closet - c3).sqrMagnitude;
        if(magSq1 < magSq2 && magSq1 < magSq3)
        {
            return c1;
        }
        else if (magSq2 < magSq1 && magSq2 < magSq3)
        {
            return c2;
        }
        return c3;
    }

    //http://realtimecollisiondetection.net/blog/?p=103
    //http://www.realtimerendering.com/intersections.html
    //https://github.com/gszauer/GamePhysicsCookbook
    public static bool DetectSphereTriangle(Vector3 pos, float radius, Triangle tri, out PhysicContact contact)
    {
        var con = new PhysicContact();
        contact = con;

        var closet = ClosetPoint(tri, pos);
        var magSq = (closet - pos).sqrMagnitude;
        if(magSq <= radius * radius)
        {
            con.ContactPos = closet;
            con.Normal = Vector3.Cross(tri.b - tri.a, tri.c - tri.a).normalized;
            con.PenetrationDepth = radius - (pos - closet).magnitude;//插入深度
            contact = con;
            return true;
        }
        return false;
    }
}
